#include "ros/ros.h"
#include "std_msgs/String.h"
#include "sensor_msgs/LaserScan.h"
#include "ans_rangefinderroscpp/obstacle.h"
#include "ans_rangefinderroscpp/obstacles.h"
#include <iostream>
#include <queue>
#include <vector> 

#define QUEUE_SIZE 10
#define MAX_LASER_RANGE 1.00
#define SCAN_RANGE 3.14         // In radians

using namespace std; 

ros::Publisher pub; 

void chatterCallback(const sensor_msgs::LaserScan::ConstPtr& scan)
{
    float angleRange = scan->angle_max - scan->angle_min; 
    float centerAngle = (scan->angle_max + scan->angle_min)/2;
    int totalSamples = angleRange/scan->angle_increment; 
    int centerSample = totalSamples/2; 
    int relevantSamples = SCAN_RANGE/scan->angle_increment; 
    int ignoreWindow = (totalSamples-relevantSamples)/2;

    cout << "Increment: " << scan->angle_increment << " " << "TotalSampls: " << relevantSamples << " MinAng: " << scan->angle_min << " MaxAng: " << scan->angle_max << " RangeAng: " << angleRange << endl; 
 
    bool * obsArr = new bool[relevantSamples]; 
    float * angleArr = new float[relevantSamples]; 
    
    // Initializes the obstacle and angle arrays
    for(int i=0; i<relevantSamples; i++)
    {
        obsArr[i] = false; 
        angleArr[i] = scan->angle_min + (i+ignoreWindow)*scan->angle_increment;
    }

    
    // Determines what portions of the scanned area have obstacles
    
    for(int i=0; i<relevantSamples; i++)
    {
        if((scan->ranges[i+ignoreWindow] < MAX_LASER_RANGE) && (scan->ranges[i+ignoreWindow] > 0.02)){
            obsArr[i] = true;
        }
    }
    //*/
   
    // Makes vector of obstacles
    //vector<ans_rangefinderroscpp::obstacle> myObstacles;
    ans_rangefinderroscpp::obstacles myObstacles;
    
    bool edgeDetect = false; 
    //obstacle * tempObs = NULL; 
    ans_rangefinderroscpp::obstacle tempObject;
    ans_rangefinderroscpp::obstacle* tempObs = &tempObject;
   
    int plusCount = 0;  // Counts how many samples in an obstacle are valid 
    float minDist = MAX_LASER_RANGE; 
    for(int i=0; i<relevantSamples; i++)
    {
        if(obsArr[i])
        {
            plusCount++; 
            tempObs = new ans_rangefinderroscpp::obstacle; 
            tempObs->startAngle = angleArr[i];
            minDist = MAX_LASER_RANGE; 
            while(obsArr[i] && (i<relevantSamples))
            {
                plusCount++; 
                i++;
                if(minDist > scan->ranges[i+ignoreWindow])
                    minDist = scan->ranges[i+ignoreWindow];
            }
            tempObs->endAngle = angleArr[i-1]; 
            tempObs->distance = minDist; 
            if(plusCount > 2)
            {
                myObstacles.obstacles.push_back(*tempObs); 
                for(int j=0; j<plusCount; j++)
                    cout << "+";  
            }else{
                delete(tempObs);
                for(int j=0; j<plusCount; j++)
                    cout << "_";  
            }    
            plusCount = 0; 
        }else{
            cout << "_"; 
        }
        
        
    }
   
    cout << endl;  
    for(int i=0; i<myObstacles.obstacles.size(); i++)
    {
        cout << i << "\t" << myObstacles.obstacles[i].startAngle*180/3.14 << "\t" << myObstacles.obstacles[i].endAngle*180/3.14 << "\t" << myObstacles.obstacles[i].distance << endl;
    }
  
    pub.publish(myObstacles);   // Publishes list of obstacles
    cout << endl << "NumOfObs: " << myObstacles.obstacles.size() << endl;
    //cout << "before delete" << endl;  
    delete(obsArr); 
    delete(angleArr); 
    //cout << "after delete" << endl;    
    myObstacles.obstacles.clear();  
}

int main(int argc, char **argv)
{
  ros::init(argc, argv, "listener");

  ROS_INFO("Main Entered");
  ros::NodeHandle n;

  ros::Subscriber sub = n.subscribe("/scan", 1, chatterCallback);
  //ros::Publisher pub = n.advertise<std_msgs::String>("obs", 1);
  pub = n.advertise<ans_rangefinderroscpp::obstacles>("obs", 1);

  ros::spin();
  return 0;
}
